COMMENT ⊗   VALID 00010 PAGES
C REC  PAGE   DESCRIPTION
C00001 00001
C00002 00002	BEGIN "QBALL"
C00005 00003	α QUE-BALLS
C00007 00004	SUBR INIT
C00008 00005	REAL SUBR DOTVEC(ITG I,J)	α DOT PERCEIVED CAMERA VECTORS
C00010 00006	SUBR LSCAM (ITG I,K,J)		α LOCUS SOLVER
C00012 00007	SUBR MKROT
C00013 00008	SUBR PROJECT(REAL FOCAL)	α SIMULATED CAMERA PROJECTION
C00014 00009	SUBR UNPROJECT(REAL FOCAL)
C00015 00010	α MAIN EXECUTION
C00032 ENDMK
C⊗;
BEGIN "QBALL"
	REQUIRE "ABBREV[SYS,BGB]" SOURCE_FILE;
	REQUIRE "SAITRG[SYS,BGB]" SOURCE_FILE;
	REQUIRE "DPYIII[SYS,BGB]" SOURCE_FILE;
	SAFE ITG ARRAY DPYBUF[0:1500];

α CAMERA;
	REAL PAN,TILT,SWING;	α CAMERA ORIENTATION;

	REAL CX,CY,CZ;		α CAMERA LOCATION;
	REAL ARRAY CLX,CLY,CLZ[1:16];ITG CLI;	α CAMERA SOLUTIONS;
	REAL PDX,PDY,FOCAL;	α PIXEL SIZE & LENS FOCAL LENGTH;
	REAL ASPECT,MAG;

	REAL RPA,CPA;		α IMAGE LOCUS OF PRINCIPLE RAY;

	REAL IX,IY,IZ;
	REAL JX,JY,JZ;
	REAL KX,KY,KZ;

α IRON TRIANGLE - CAMERA LOCUS SOLVER;

	REAL ARRAY P1,P2,P3,COSANG[1:3],V[1:10,1:3];
	REQUIRE "LS1V3P.REL" LOAD_MODULE;
	EXTERNAL ITG PROCEDURE LS1V3P(REAL ARRAY V,P1,P2,P3,COSANG);

α QUE-BALLS;

	REAL ARRAY XWC,YWC,ZWC[1:16];		α WORLD COORDINATES;

	REAL ARRAY XCC,YCC,ZCC[1:16];		α PREDICTED CAMERA COORDINATES;
	REAL ARRAY XPP,YPP,ZPP[1:16];		α PREDICTED IMAGE  COORDINATES;
	REAL ARRAY XDC,YDC,ZDC[1:16];		α DISPLAY COORDINATES;

	REAL ARRAY PRROW,PRCOL[1:16];		α PERCEIVED ROW & COL;
	REAL ARRAY PRXCC,PRYCC,PRZCC[1:16];	α PERCEIVED CAMERA COORDINATES;
	REAL ARRAY PRXPP,PRYPP[1:16];		α PERCEIVED IMAGE  COORDINATES;
	REAL ARRAY PRXDC,PRYDC[1:16];		α PERCEIVED DISPLAY COORDINATES;

	REAL C,S;ITG I;
SUBR INIT;
BEGIN "INIT"

α PRINCIPLE RAY LOCUS IN CAMERA'S IMAGE PLANE;
	RPA ← 108;	CPA ← 144;

α SIMULATED CAMERA LOCUS;
	CX ← 10;	CY ← -30;	CZ ← 20;

α SIMULATED CAMERA ORIENTATION;
	PAN  ← 14*π/180;	TILT ← 57*π/180;	SWING ← 5*π/180;
	MAG ← 32/9;
	FOCAL ← 21000;	
	PDX ← 38.78;	PDY ← 40.0;

END "INIT";


SUBR ORBINIT(REAL RADIUS);		α INITIALIZE WORLD POINTS;
BEGIN "ORBINIT"

	C ← COS(-π/8);	S ← SIN(-π/8);
	XWC[1] ← RADIUS;
	YWC[1] ← ZWC[1] ← 0;
	FOR I←2 THRU 16 DO
	⊂ XWC[I] ← C*XWC[I-1] - S*YWC[I-1];
	  YWC[I] ← S*XWC[I-1] + C*YWC[I-1];
	  ZWC[I]←0; ⊃;

END "ORBINIT";
REAL SUBR DOTVEC(ITG I,J);	α DOT PERCEIVED CAMERA VECTORS;
BEGIN "DOTVEC"
	REAL X1,Y1,Z1,X2,Y2,Z2,R1,R2,ZCOS;
	X1 ← PRXCC[I]; Y1 ← PRYCC[I]; Z1 ← PRZCC[I];
	X2 ← PRXCC[J]; Y2 ← PRYCC[J]; Z2 ← PRZCC[J];
	R1 ← SQRT(X1*X1 + Y1*Y1 + Z1*Z1);
	R2 ← SQRT(X2*X2 + Y2*Y2 + Z2*Z2);
	ZCOS←(X1*X2 + Y1*Y2 + Z1*Z2)  /  (R1*R2);
	RETURN(ZCOS);
END "DOTVEC";

SUBR LSCAM (ITG I,K,J);		α LOCUS SOLVER;
BEGIN "LSCAM"
	ITG M,N,L;
	REAL QMIN,Q;

α IRON TRIANGLE - KNOWN WORLD LOCI;
	P1[1]←XWC[I]; P2[1]←XWC[J]; P3[1]←XWC[K];
	P1[2]←YWC[I]; P2[2]←YWC[J]; P3[2]←YWC[K];
	P1[3]←ZWC[I]; P2[3]←ZWC[J]; P3[3]←ZWC[K];

α IRON TRIPOD - KNOW ANGLES BETWEEN  CAMERA RAYS;
	COSANG[1] ← DOTVEC(J,K);
	COSANG[2] ← DOTVEC(I,K);
	COSANG[3] ← DOTVEC(I,J);

α THROW THE SHIT AT THE FAN;
	M ← LS1V3P(V,P1,P2,P3,COSANG);
	QMIN ← 999999;

	FOR N←1 THRU M DO
BEGIN	
	Q ← SQRT ((V[N,1]-CX)↑2+(V[N,2]-CY)↑2+(V[N,3]-CZ)↑2);
	IF Q<QMIN THEN ⊂ QMIN←Q;L←N;⊃;
END;	

α	OUTSTR(9&CVG(V[L,1])&9&CVG(V[L,2])&9&CVG(V[L,3])&↓);

α CAMERA LOCUS SOLUTIONS;
	CLX[CLI]←V[L,1];
	CLY[CLI]←V[L,2];
	CLZ[CLI]←V[L,3];
	CLI ← CLI+1;
END "LSCAM";
SUBR MKROT;
BEGIN "MKROT"
	REAL RR;
	REAL C_PAN,S_PAN,C_TILT,S_TILT,C_SWING,S_SWING;

	C_PAN ← COS(PAN); S_PAN ← SIN(PAN);
	C_TILT ← COS(TILT); S_TILT ← SIN(TILT);
	C_SWING ← COS(SWING); S_SWING ← SIN(SWING);

	IX ←  C_PAN*C_SWING - S_PAN*C_TILT*S_SWING;
	IY ←  S_PAN*C_SWING + C_PAN*C_TILT*S_SWING;
	IZ ←  S_TILT*S_SWING;

	JX ← -C_PAN*S_SWING - S_PAN*C_TILT*C_SWING;
	JY ← -S_PAN*S_SWING + C_PAN*C_TILT*C_SWING;
	JZ ←  S_TILT*C_SWING;

	KX ←  S_PAN*S_TILT;
	KY ← -C_PAN*S_TILT;
	KZ ←       C_TILT;

END "MKROT";
SUBR PROJECT(REAL FOCAL);	α SIMULATED CAMERA PROJECTION;
BEGIN "PROJECT"
	ITG I;

FOR I←1 THRU 16 DO
BEGIN
	REAL X,Y,Z,SX,SY;

α WC → CC WORLD LOCII PREDICTED;
	X ← XWC[I] - CX;
	Y ← YWC[I] - CY;
	Z ← ZWC[I] - CZ;
	XCC[I] ← X*IX + Y*IY + Z*IZ;
	YCC[I] ← X*JX + Y*JY + Z*JZ;
	ZCC[I] ← X*KX + Y*KY + Z*KZ;

α CC → PP;
	SX ← -FOCAL/PDX;
	SY ← -FOCAL/PDY;
 	XPP[I] ← SX * XCC[I] / ZCC[I];
	YPP[I] ← SY * YCC[I] / ZCC[I];

α PP → DC;
	XDC[I] ← MAG * (XPP[I]+(CPA-144));
	YDC[I] ← MAG * (YPP[I]+(RPA-108));

END;
END "PROJECT";
SUBR UNPROJECT(REAL FOCAL);
BEGIN "UNPROJECT"
	
	ITG I;
	FOR I←1 THRU 16 DO
BEGIN
	PRXCC[I] ← XPP[I]*PDX;
	PRYCC[I] ← YPP[I]*PDY;
	PRZCC[I] ← -FOCAL;
END;
END "UNPROJECT";
α MAIN EXECUTION;

	INIT;
	ORBINIT(6);
	MKROT;
	PROJECT(FOCAL);

WHILE TRUE DO
BEGIN "MAIN"
	ITG I;ITG ARRAY DPYBUF[1:1000];
	REAL X0,Y0,Z0,X1,Y1,Z1;
	REAL SDX,SDY,SDZ,DEL,DEL2,SD;
	DPYSET(DPYBUF);
	AIVECT(0,-500);AVECT(0,500);
	AIVECT(500,-500);AVECT(-500,-500);

FOR DEL ← -15000 STEP 1000 UNTIL 15000 DO
BEGIN "LOOP"
	UNPROJECT(FOCAL+DEL);CLI ← 1;
	LSCAM(6,1,11);LSCAM(5,2,13);LSCAM(4,3,9);
	LSCAM(10,4,16);LSCAM(9,8,3);LSCAM(14,10,6);
α AVERAGE SOLUTION;
	X0←Y0←Z0←X1←Y1←Z1←0;
	FOR I←1 THRU 6 DO
	⊂ X0←X0+CLX[I];Y0←Y0+CLY[I];Z0←Z0+CLZ[I];
	  X1←X1+CLX[I]↑2;Y1←Y1+CLY[I]↑2;Z1←Z1+CLZ[I]↑2;⊃;
	X0←X0/6; Y0←Y0/6; Z0←Z0/6;X1←X1/6; Y1←Y1/6; Z1←Z1/6;

α STANDARD DEVIATIONS;
	SDX ← SQRT(X1 - X0↑2);SDY ← SQRT(Y1 - Y0↑2);SDZ ← SQRT(Z1 - Z0↑2);
	SD  ← SQRT(SDX↑2 + SDY↑2 + SDZ↑2);
	AVECT(DEL*500/15000,50*SD-500);DPYOUT(1);
	OUTSTR(9&CVS(DEL)&9&CVG(SD)&↓);
END "LOOP";
	AVECT(500,-500);DPYOUT(1);AIVECT(-500,0);
	INCHRW;
END "MAIN";

END "QBALL";